#include "Nano100Series.h"
#include "HmdCmd.h"
#include "Common.h"
#include "string.h"


#define HMD_CMD_DISP_XFER		(0x03)

#define HMD_CMD_LENGTH_MAX		(63)

#define HMD_CMD_DISP_XFER_SYNC_BEGIN			(0x55)
#define HMD_CMD_DISP_XFER_SYNC_END				(0xAA)

#define HMD_CMD_DISP_XFER_RESULT_OK				(0)
#define HMD_CMD_DISP_XFER_RESULT_ERROR			(-1)

#define HMD_CMD_DISP_XFER_GET_CONTRAST				(0x00)
#define HMD_CMD_DISP_XFER_SET_CONTRAST				(0x01)
#define HMD_CMD_DISP_XFER_GET_BRIGHTNESS			(0x02)
#define HMD_CMD_DISP_XFER_SET_BRIGHTNESS			(0x03)
#define HMD_CMD_DISP_XFER_GET_SATURATION			(0x04)
#define HMD_CMD_DISP_XFER_SET_SATURATION			(0x05)
#define HMD_CMD_DISP_XFER_GET_3D					(0x06)
#define HMD_CMD_DISP_XFER_SET_3D					(0x07)
#define HMD_CMD_DISP_XFER_GET_VIDEO_STATE			(0x08)
#define HMD_CMD_DISP_XFER_GET_VERSION				(0x09)
#define HMD_CMD_DISP_XFER_GET_BRIGHTNESS_OFFSET		(0x0A)
#define HMD_CMD_DISP_XFER_SET_BRIGHTNESS_OFFSET		(0x0B)
#define HMD_CMD_DISP_XFER_GET_BRIGHTNESS_ABS		(0x0C)
#define HMD_CMD_DISP_XFER_SET_BRIGHTNESS_ABS		(0x0D)

#define HMD_PARAM_DISP_3D_NONE			(0x00)
#define HMD_PARAM_DISP_3D_HALF_LR		(0x01)
#define HMD_PARAM_DISP_3D_HALF_TB		(0x02)
#define HMD_PARAM_DISP_3D_FULL			(0x03)

#define HMD_PARAM_DISP_VIDEO_STATE_OFF	(0x00)
#define HMD_PARAM_DISP_VIDEO_STATE_ON	(0x01)

#define FRACTOR_SCALE_4		(10000)


void HmdRspNull(uint8_t *cmd_buf, uint32_t cmd_size, uint8_t *rsp_buf, uint32_t rsp_size, uint32_t *rsp_ret_size)
{
	*rsp_ret_size = 0;
}

short calcChkSum(uint8_t *buffer, int byteOffset, int byteCount)
{
	short chkSum = 0;
	int byteEnd;
	int i;

	byteEnd = byteOffset + byteCount;
	for (i = byteOffset; i < byteEnd; i++) {
		chkSum += (short)(buffer[i] & 0x00FF);
	}
	return chkSum;
}

int pack(uint8_t *packedBuf, int packedCount, uint8_t *payloadBuf, int payloadOffset, int payloadCount) {
	int index;
	short chkSum;
	int i;

	if (packedCount < (payloadCount + 6)) {
		return -1;
	}

	index = 0;
	packedBuf[index] = HMD_CMD_DISP_XFER_SYNC_BEGIN;
	index++;
	packedBuf[index] = (payloadCount & 0x0000FF00) >> 8;
	index++;
	packedBuf[index] = (payloadCount & 0x000000FF);
	index++;
	for (i = 0; i < payloadCount; i++, index++) {
		packedBuf[index] = payloadBuf[payloadOffset + i];
	}
	chkSum = calcChkSum(packedBuf, 1, payloadCount + 2);
	packedBuf[index] = (chkSum & 0xFF00) >> 8;
	index++;
	packedBuf[index] = chkSum & 0x00FF;
	index++;
	packedBuf[index] = HMD_CMD_DISP_XFER_SYNC_END;
	index++;

	return index;
}

int unpack(uint8_t *packedBuf, int packedCount, uint8_t *payloadBuf, int payloadOffset, int payloadCount) {
	int index;
	short length;
	short chkSumRecv, chkSumCalc;
	int i;

	index = 0;
	if (packedBuf[index] != HMD_CMD_DISP_XFER_SYNC_BEGIN) {
		return -1;
	}
	index++;
	length = (short)((packedBuf[index] & 0x00FF) << 8);
	index++;
	length |= (short)(packedBuf[index] & 0x00FF);
	index++;
	if (packedCount != (length + 6)) {
		return -1;
	}
	if (payloadCount < length) {
		return -1;
	}
	for (i = 0; i < length; i++, index++) {
		payloadBuf[payloadOffset + i] = packedBuf[index];
	}
	chkSumCalc = calcChkSum(packedBuf, 1, length + 2);

	chkSumRecv = (short)((packedBuf[index] & 0x00FF) << 8);
	index++;
	chkSumRecv |= (short)(packedBuf[index] & 0x00FF);
	index++;

	if (chkSumRecv != chkSumCalc) {
		return -1;
	}
	if (packedBuf[index] != HMD_CMD_DISP_XFER_SYNC_END) {
		return -1;
	}

	return length;
}

int HmdDispRspXferError(uint8_t *sendCmdBuf, int sendCmdCount)
{
	int i;

	i = 0;
	sendCmdBuf[i] = HMD_CMD_DISP_XFER_RESULT_ERROR & 0xFF;
	i++;

	return i;
}

int HmdDispRspXferGetVersion(uint8_t *sendCmdBuf, int sendCmdCount, uint8_t *recvCmdBuf, int recvCmdCount)
{
	int i;
	int j;
	int n;

	i = 0;
	sendCmdBuf[i] = HMD_CMD_DISP_XFER_RESULT_OK;
	i++;
	n = strlen(version);
	for (j = 0; j < n; j++) {
		sendCmdBuf[i++] = version[j];
	}

	return i;
}

void HmdDispRspXfer(uint8_t *cmd_buf, uint32_t cmd_size, uint8_t *rsp_buf, uint32_t rsp_size, uint32_t *rsp_ret_size)
{
	uint8_t sendCmdBuf[HMD_CMD_LENGTH_MAX - 6];
	int sendCmdCount;
	uint8_t recvCmdBuf[HMD_CMD_LENGTH_MAX - 6];
	int recvCmdCount;

	recvCmdCount = unpack(cmd_buf + 1, cmd_size - 1, recvCmdBuf, 0, sizeof(recvCmdBuf));
	sendCmdCount = HmdDispRspXferError(rsp_buf, sizeof(rsp_size));

	if (recvCmdCount < 0) {

	} else {
		switch (recvCmdBuf[0]) {
			case HMD_CMD_DISP_XFER_GET_VERSION :
				sendCmdCount = HmdDispRspXferGetVersion(sendCmdBuf, sizeof(sendCmdBuf), recvCmdBuf, recvCmdCount);
				break;
			default:
				break;
		}
	}

	*rsp_ret_size = pack(rsp_buf, rsp_size, sendCmdBuf, 0, sendCmdCount);
}
